#include <avr/interrupt.h>
#include <avr/pgmspace.h>
#include <string.h>
#include <stdint.h>

#include "dx6i.h"
#include "rxtx.h"
#include "conf.h"

volatile cnt_t cnt;
volatile beep_t beep;
extern conf_t conf;

void dx6i_init(void){
	memset((void*)&cnt, 0, sizeof(cnt));
	read_conf();

	LCD_INIT
	VIBRO_INIT
	BEEP_IN_INIT
	BEEP_OUT_INIT

	TCCR2A = (1 << WGM21);
	TCCR2B = (1 << CS21);
	OCR2A = (F_CPU / 8 / CNT_FREQ / 2 - 1);
	TIMSK2|= (1 << OCIE2A);

	rxtx_init();

	if(conf.f & (1 << CB_BEEP_LCD))
		lcd_on();
}

ISR(INT0_vect){
	cnt.aft = 0;

	if(BEEP_IN_HIGH){
		if(conf.f & (1 << CB_BEEP_BEEP))
			BEEP_ON

		if(conf.f & (1 << CB_BEEP_LCD))
			lcd_on();

		if(conf.f & (1 << CB_BEEP_VIBRO))
			vibro_on(true);

	}else
		BEEP_OFF
}

ISR(TIMER2_COMPA_vect){
	sei();

	isr_beep();
	isr_lcd();
	isr_vibro();
	isr_aft();

	if(cnt.rx < DSM2_RX_DETECT_CNT)
		cnt.rx++;

	if(cnt.conf)
		cnt.conf--;
}

static void isr_beep(void){
	if(!cnt.beeps)
		return;

	if(++cnt.beep < beep.on){
		BEEP_TOGGLE
	}else{
		if(cnt.beep == beep.on){
			BEEP_OFF
		}else if(cnt.beep == beep.off){
			cnt.beeps--;
			cnt.beep = 0;
		}
	}
}

static void isr_lcd(void){
	if(cnt.lcd)
		cnt.lcd--;

	if(!cnt.pwm_lcd_val){
		LCD_OFF
		return;
	}

	if(cnt.pwm_lcd++ >= PWM_LCD_CNT){
		cnt.pwm_lcd = 0;
		LCD_ON
	}else if(cnt.pwm_lcd == cnt.pwm_lcd_val){
		if(cnt.pwm_lcd_val != PWM_LCD_CNT)
			LCD_OFF
		if(!cnt.lcd){
			if(conf.f & (1 << CB_LCD_DIMM))
				cnt.pwm_lcd_val--;
			else
				cnt.pwm_lcd_val = 0;
		}
	}
}

static void isr_vibro(void){
	if(cnt.vibro){
		cnt.vibro--;
		if(cnt.vibro_detect < conf.vibro_detect)
			cnt.vibro_detect++;
	}else
		cnt.vibro_detect = 0;

	if(++cnt.pwm_vibro >= PWM_VIBRO_CNT){
		cnt.pwm_vibro = 0;
		if(cnt.vibro && cnt.pwm_vibro_val && cnt.vibro_detect >= conf.vibro_detect)
			VIBRO_ON
		else
			VIBRO_OFF
	}else if(cnt.pwm_vibro == cnt.pwm_vibro_val){
		VIBRO_OFF
		if(!cnt.vibro)
			cnt.pwm_vibro_val = 0;
	}
}

static void isr_aft(void){
	if(!conf.aft)
		return;

	if(cnt.aft < conf.aft){
		cnt.aft++;
		return;
	}

	if(cnt.aft == conf.aft || (!cnt.aft_repeat && conf.aft_repeat)){
		if(cnt.aft == conf.aft)
			cnt.aft++;

		if(conf.f & (1 << CB_AFT_BEEP))
			beep_on(3);

		if(conf.f & (1 << CB_AFT_LCD))
			lcd_on();

		if(conf.f & (1 << CB_AFT_VIBRO))
			vibro_on(false);

		cnt.aft_repeat = conf.aft_repeat;
	}

	if(cnt.aft_repeat)
		cnt.aft_repeat--;
}

void beep_on(uint8_t beeps){
	cnt.beep = 0;
	cnt.beeps = beeps;

	beep.on = CNT_MSEC * 110;
	beep.off = beep.on + CNT_MSEC * 10;
}

void lcd_on(void){
	if(!conf.lcd)
		return;

	cnt.lcd = conf.lcd;
	cnt.pwm_lcd_val = conf.lcd_power;
}

void vibro_on(bool detect){
	if(!conf.vibro)
		return;

	if(!detect){
		cnt.vibro = conf.vibro_aft;
		cnt.vibro_detect = conf.vibro_detect;
	}else
		cnt.vibro = conf.vibro;
	cnt.pwm_vibro_val = conf.vibro_power;
}
